"""基于论文Crater Identification Algorithm for the Lost in Low Lunar Orbit Scenario
"""

from .base import TriadPlainBuilder
from utils.ellipse import ellipse_args
import numpy as np


class HanakBuilder(TriadPlainBuilder):
    def invariant(self, i, j, k, Q1, Q2, Q3) -> tuple[np.ndarray, np.ndarray]:
        N = Q3.shape[0]
        Q1 = Q1[None].repeat(N, axis=0)
        Q2 = Q2[None].repeat(N, axis=0)
        x1, y1, a1, b1, theta1 = ellipse_args(Q1)
        x2, y2, a2, b2, theta2 = ellipse_args(Q2)
        x3, y3, a3, b3, theta3 = ellipse_args(Q3)
        d1 = a1 + b1
        d2 = a2 + b2
        d3 = a3 + b3
        ds = np.array((d1, d2, d3))
        # 求解椭圆中心构成的三角形中两个较大的内角
        # 三角形的三个顶点坐标
        A = np.pad((x1, y1), ((0, 1), (0, 0)), constant_values=1)
        B = np.pad((x2, y2), ((0, 1), (0, 0)), constant_values=1)
        Cs = np.pad((x3, y3), ((0, 1), (0, 0)), constant_values=1)
        ABC = np.array((A, B, Cs)).T
        # 三角形的三条边长
        a = np.linalg.norm(Cs - B, axis=0)
        b = np.linalg.norm(A - Cs, axis=0)
        c = np.linalg.norm(A - B, axis=0)
        l = np.max((a, b, c), axis=0)
        # 三角形的三个内角
        cosA = (b**2 + c**2 - a**2) / (2 * b * c)
        cosB = (a**2 + c**2 - b**2) / (2 * a * c)
        cosC = (a**2 + b**2 - c**2) / (2 * a * b)
        cos = np.array((cosA, cosB, cosC))
        # 按角度从大到小排序，即余弦值从小到大排序
        index = np.argsort(cos, axis=0)
        # 排除共线的情况，即三个点构成的行列式为0
        sequence = np.linalg.det(ABC)
        remain_ind = ~np.isclose(sequence, 0)
        # 排除椭圆大小重叠、不同椭圆交叉的情况
        remain_ind &= (c > (d1 + d2) / 2) & (b > (d1 + d3) / 2) & (a > (d2 + d3) / 2)
        # 三角形的两个较大内角，对应较小的两条边
        idx = np.arange(N)
        # 按原始顺序排序输出：三个对应椭圆半径与最大边长之比，最大内角、第二大内角的余弦值，以及按角度最大、第二大和最小顺序的旋转方向。
        ijk = np.vstack((np.ones_like(k) * i, np.ones_like(k) * j, k))
        ijk = np.take_along_axis(ijk, index, axis=0)
        # 求解不变量
        I = np.vstack(
            (
                ds[index, idx] / l,
                cos[index[0], idx],
                cos[index[1], idx],
                np.sign(sequence),
            )
        ).astype(np.float32)
        return ijk[:, remain_ind], I[:, remain_ind]
